#include "path_planning/path_planner.h"

WaypointList::WaypointList() :
	first_(NULL),
	current_(NULL),
	end_of_list_(false){};

WaypointList::~WaypointList(){
	Node* aux = first_;

	Node* next_node;
	while(aux){
		next_node = aux->next;

		delete aux->point;
		aux->point = NULL;

		delete aux;
		aux = next_node;
	}

	first_ = NULL;
	last_= NULL;
	current_= NULL;
};

bool WaypointList::Empty() const{
	return first_ == NULL;
};

player_point_2d* WaypointList::Head() const{
	return (first_ == NULL) ? NULL : first_->point;
};

player_point_2d* WaypointList::Next(){

	if(end_of_list_ == true)
		return NULL;

	if(current_ == NULL)
		current_ = first_;
	else
		current_ = current_->next;

	end_of_list_ = !current_ && first_ ? true : false;

	return current_ != NULL ? current_->point : NULL;
};

player_point_2d* WaypointList::Previous(){

	if(current_ == NULL && end_of_list_ == true){
		current_ = last_;
		end_of_list_ = false;
	}
	else if(current_ != NULL)
		current_ = current_->previous;

	return current_ != NULL ? current_->point : NULL;
};

player_point_2d* WaypointList::Add(double x_coordinate, double y_coordinate){

	player_point_2d* new_point = new player_point_2d();
	new_point->px = x_coordinate;
	new_point->py = y_coordinate;

	if(!first_){
		first_ = new Node();
		first_->next = NULL;
		last_ = first_;
	}else{
		Node* aux = new Node();
		aux->next = first_;
		first_->previous = aux;
		first_ = aux;
	}

	first_->previous = NULL;
	first_->point = new_point;

	return new_point;
};


player_point_2d* WaypointList::Remove(player_point_2d* point){

	Node* aux = first_;

	player_point_2d* point_to_remove;
	while(aux){
		if(aux->point == point && aux == first_)
		{
			point_to_remove = aux->point;
			aux = aux->next;

			if(aux)
				aux->previous = NULL;
			else
				last_ = NULL;

			delete first_;
			first_ = aux;

			return point_to_remove;
		}
		else if(aux->point == point && aux == last_)
		{
			point_to_remove = aux->point;

			aux = aux->previous;
			aux->next = NULL;

			delete last_;
			last_ = aux;

			return point_to_remove;
		}else if(aux->point == point)
		{
			point_to_remove =  aux->point;

			aux->previous->next = aux->next;
			aux->next->previous = aux->previous;

			delete aux;
			aux = NULL;

			return point_to_remove;
		}
		aux = aux->next;
	}
	//could not find node, return NULL
	return NULL;
};

void WaypointList::Reset(){

	//Restart list state
	current_ = NULL;
	end_of_list_ = false;
}

PathPlanner::PathPlanner(string grid_map_path_file, string topological_map_file_descriptor) : open_avl_tree_(NULL), closed_avl_tree_(NULL){
	cout<<"CALL PathPlanner::Constructor: -"<<endl;

	map_ = new PlannerMap();

    cout << "PathPlanner::Constructor: Setting topological map. File descriptor = " << topological_map_file_descriptor << " -"<<endl;
	if(!map_->SetGraphDescriptor(topological_map_file_descriptor))
		cerr << "PathPlanner::Constructor: Error when trying to parse graph descriptor. Nok" << endl;

	cout<<"PathPlanner::Constructor: Setting grid map. File descriptor = "<<grid_map_path_file<<" -" << endl;
		if(!map_->SetGridMapDescriptor(grid_map_path_file))
			cerr << "PathPlanner::Constructor: Error when trying to parse grid map descriptor. Nok" << endl;

	cout<<"PathPlanner::Constructor. End"<<endl;
}


WaypointList* PathPlanner::RequestWaypoints(player_point_2d origin, player_point_2d goal){
	cout<<"CALL PathPlanner::RequestWaypoints:  -"<<endl;

	if(!map_){
		cerr<<"Planner::RequestWaypoints: Error: NULL pointer to map reference. END"<<endl;
		return NULL;
	}

	Clean();

	open_avl_tree_ = new AvlTree();
	closed_avl_tree_ = new AvlTree();

	Vertex* closest_vertex_to_origin = map_->GetClosestVertex(origin.px, origin.py);
	Vertex* closest_vertex_to_goal = map_->GetClosestVertex(goal.px, goal.py);

	closest_vertex_to_origin->g = 0;
	closest_vertex_to_origin->h = INFINITY;
	closest_vertex_to_origin->f = INFINITY;

	start_ = closest_vertex_to_origin;
	target_ = closest_vertex_to_goal;

	open_avl_tree_->Insert(new Comparable(start_->id, start_->f));
	bool found_goal=false;
	Comparable *current_avl_node;
	while(!found_goal && !open_avl_tree_->IsEmpty()){
		current_avl_node = open_avl_tree_->PickNode();
		if(current_avl_node->Key().vertex_id == target_->id){
			found_goal = true;
		}else{
			closed_avl_tree_->Insert(current_avl_node);
			ParseAdjacents(current_avl_node->Key().vertex_id);
		}
	}

	if(!found_goal){
		cerr<<"PathPlanner::RequestWaypoints: Could not find a route. NOk. END"<<endl;
		return NULL;
	}

    Vertex *aux_vertex= map_->GetVertex(current_avl_node->Key().vertex_id);

    WaypointList* list = new WaypointList();
    while(aux_vertex){
    	list->Add(aux_vertex->x_coordinate, aux_vertex->y_coordinate);
    	aux_vertex = aux_vertex->parent;
    }

    cout<<"PathPlanner::RequestWaypoints: Ok. END"<<endl;
	return list;
}

WaypointList* PathPlanner::RequestAlternativeRoute(player_point_2d current_position, player_point_2d goal_position,
											player_point_2d previous_node_position, player_point_2d next_node_position ){
	cout<<"CALL PathPlanner::RequestAlternativeRoute:  -"<<endl;

	Vertex* previous = map_->GetClosestVertex(previous_node_position.px, previous_node_position.py);
	Vertex* next = map_->GetClosestVertex(next_node_position.px, next_node_position.py);

	map_->RemoveEdge(previous->id, next->id);

	WaypointList* route = RequestWaypoints(current_position, goal_position);

	map_->AddEdge(previous->id, next->id);

	if(route == NULL)
		cerr<<"PathPlanner::RequestAlternativeRoute: Failure. An error occurred when trying to set alternative route. Nok. END"<<endl;
	else
		cout<<"PathPlanner::RequestAlternativeRoute. Ok. END"<<endl;

	return route;
}

void PathPlanner::SearchForShortcut(Vertex* current, double edge_cost, Vertex* adjacent){
	if(current->parent != adjacent->parent)
		return;
	if(adjacent->g > current->g + edge_cost){
		adjacent->parent = current;
		adjacent->g = current->g + edge_cost;
		adjacent->f = adjacent->g + adjacent->h;
	}
}

void PathPlanner::CalculateFGH(Vertex* adjacent, double edge_cost, Vertex* goal){
		adjacent->h = sqrt(pow((goal->x_coordinate - adjacent->x_coordinate), 2) + pow((goal->y_coordinate - adjacent->y_coordinate), 2));
		adjacent->g = adjacent->parent->g + edge_cost;
		adjacent->f = (adjacent->h + adjacent->g);
}


void PathPlanner::ParseAdjacents(int current_id){
	bool bopen = false, bclosed = false, found_goal = false;

	AdjNodeList *list_of_adjacents = map_->GetAdjacents(current_id);

	AdjNode *adjacent_identification = list_of_adjacents->GetFirst();
	Vertex* current = map_->GetVertex(current_id);
	while(!found_goal && adjacent_identification){
		Vertex *adjacent = map_->GetVertex(adjacent_identification->adjacent_id);

		AvlKey key;
		key.vertex_id = adjacent->id;
		key.f = adjacent->f;

		bopen = open_avl_tree_->Search(key);
		bclosed = closed_avl_tree_->Search(key);

		if(!bclosed  && !bopen){
			adjacent->parent = current;

			CalculateFGH(adjacent, adjacent_identification->dist, target_);
			key.f = adjacent->f;
			while(open_avl_tree_->Search(key)){
				key.f += 0.0000001;
				adjacent->f = key.f;
			}

			open_avl_tree_->Insert(new Comparable(adjacent->id, adjacent->f));
		}else if(bopen){
			SearchForShortcut(current, adjacent_identification->dist, adjacent);
		}
		adjacent_identification = adjacent_identification->next;
	}
}

void PathPlanner::Clean(){
	map_->Reset();

	if(open_avl_tree_){
		delete open_avl_tree_;
		open_avl_tree_ = NULL;
	}

	if(closed_avl_tree_){
		delete closed_avl_tree_;
		closed_avl_tree_ = NULL;
	}
}

